#include "RigidBody.h"

using namespace PHYOgre;

RigidBody::RigidBody(void)
	:mInverseInertialTensor()
	,mInverseInertialTensorWorld()
	,mLinearDampling(DEFAULT_DAMPLING)
	,mAngularDampling(DEFAULT_DAMPLING)
	,mForceAccumulator(Vector3::ZERO)
	,mVelocity(Vector3::ZERO)
	,mAcceleration(Vector3::ZERO)
	,mAngularVelocity(Vector3::ZERO)
	,mTorqueAccumulator(Vector3(0,200,0))
{
}


RigidBody::~RigidBody(void)
{
}

void RigidBody::CalculateDerivedData()
{
	//mTransformMatrix = Matrix4(mOrientation);
	//mTransformMatrix.setTrans(mPosition);
	UpdateInertialTensorWorld();
}
void RigidBody::SetInertialTensor(const Matrix3& iTensor)
{
	mInverseInertialTensor = iTensor.Inverse();
	UpdateInertialTensorWorld();
}

void RigidBody::UpdateInertialTensorWorld()
{
	Matrix3 toWorldOrientation;
	mNode->getOrientation().ToRotationMatrix(toWorldOrientation);
	mInverseInertialTensorWorld = toWorldOrientation.Inverse() * mInverseInertialTensor;
}

void RigidBody::SetMass(const Real& iMass)
{
	assert(iMass >=0.0f);

	if (iMass <= MIN_MASS_VALUE)
		mInverseMass = MAX_MASS_VALUE;
	else
		mInverseMass = 1.0f / iMass;

}
void RigidBody::SetNode(Ogre::Node *iNode)
{
	mNode = iNode;
}
void RigidBody::SetPosition(const Vector3& iPosition)
{
	mNode->setPosition(iPosition);
}
void RigidBody::SetInverseMass(const Real& iInverseMass)
{
	assert(iInverseMass >0.0f);
	mInverseMass = iInverseMass;
}
Real RigidBody::GetInverseMass() const
{
	return mInverseMass;
}
Vector3 RigidBody::GetVelocity() const
{
	return mVelocity;
}
Vector3 RigidBody::GetPosition() const
{
	return mNode->getPosition();
}

bool RigidBody::HasFiniteMass() const
{
	return mInverseMass == 0 ? false: true;
}
void RigidBody::AddForce(const Vector3& iForce)
{
	mForceAccumulator += iForce;
}
void RigidBody::AddForceAtBodyPoint(const Vector3& iForceWorldSpace, const Vector3& iPointLocalSpace)
{
	Vector3 pointWorldSpace = mNode->_getFullTransform() * iPointLocalSpace;
	AddForceAtPoint(iForceWorldSpace, pointWorldSpace);
}
void RigidBody::AddForceAtPoint(const Vector3& iForceWorldSpace, const Vector3& iPointWorldSpace)
{
	Vector3 distance = iPointWorldSpace - mNode->getPosition();

	mForceAccumulator	+= iForceWorldSpace;
	mTorqueAccumulator += distance.crossProduct(iForceWorldSpace);

}
void RigidBody::ClearForceAccumulator()
{
	mForceAccumulator = Vector3::ZERO;
	mTorqueAccumulator = Vector3::ZERO;
}
void RigidBody::Integrate(const Real& iDT)
{
	assert(iDT > 0.0f);

	 mLastFrameAcceleration = mAcceleration;
	 mLastFrameAcceleration += mForceAccumulator * mInverseMass;

	 Vector3 angularAcceleration = mTorqueAccumulator *mInverseInertialTensorWorld;

	 //euler 
	 mVelocity += mLastFrameAcceleration * iDT;
	 mAngularVelocity += angularAcceleration * iDT;

	 mVelocity *= Ogre::Math::Pow(mLinearDampling , iDT);
	 mAngularVelocity *= Ogre::Math::Pow(mAngularDampling , iDT);

	// Ogre::Quaternion f;
	 mNode->translate(mVelocity * iDT);
	 mShape->Translate(mVelocity * iDT);
	 //TODO complete
	 Vector3 scaledAngularVelocity = mAngularVelocity * iDT;
	 Quaternion q(1,scaledAngularVelocity.x,scaledAngularVelocity.y,scaledAngularVelocity.z);
	 mNode->rotate(q);
	 mShape->Rotate(q);

	 CalculateDerivedData();
	 ClearForceAccumulator();
}

const Ogre::Sphere& RigidBody::GetBoundingSphere() const
{
	return mObject->getWorldBoundingSphere();
}
const Ogre::MovableObject& RigidBody::GetObject() const
{
	return *mObject;
}

void RigidBody::SetShape(Shape* iShape)
{
	mShape = iShape;
}
const Shape* RigidBody::GetShape() const
{
	return mShape;
}
void RigidBody::SetFriction(const Real& iFriction)
{
	mFriction = iFriction;
}
void RigidBody::SetRestitution(const Real& iRestitution)
{
	mRestitution = iRestitution;
}

Real RigidBody::GetRestitution()const
{
	return mRestitution;
}
Real RigidBody::GetFriction()const
{
	return mFriction;
}
